import numpy as np
import matplotlib.pyplot as plt
import math
def plot(frame_idx, rewards):
    plt.figure(figsize=(5, 5))
    # plt.subplot(131)
    plt.title('frame %s. reward: %s' % (frame_idx, rewards[-1]))
    plt.plot(rewards)
    plt.show()

def slc(yaw, forward_v, lateral_v, origin_x, origin_y, hangdian2_x, hangdian2_y, boat_x, boat_y):
    yaw_rad = math.radians(yaw)

    # 计算船在指定向量方向上的速度投影
    # vector_hangdian = np.array([hangdian2_x - origin_x, hangdian2_y - origin_y])

    # v_x = np.array([forward_v * math.cos(yaw_rad), forward_v * math.sin(yaw_rad)])
    # v_y = np.array([lateral_v * math.cos(yaw_rad), lateral_v * math.sin(yaw_rad)])
    # velocity_projection = (np.dot(v_x, vector_hangdian) / math.sqrt(np.dot(vector_hangdian, vector_hangdian)) )
    #                        # + np.dot(v_y, vector_hangdian) / math.sqrt(np.dot(vector_hangdian, vector_hangdian)))

    # 计算船到指定直线的距离
    line_vector_x = hangdian2_x - origin_x
    line_vector_y = hangdian2_y - origin_y
    boat_vector_x = boat_x - origin_x
    boat_vector_y = boat_y - origin_y
    distance_to_line = (line_vector_x * boat_vector_y - line_vector_y * boat_vector_x) / math.sqrt(line_vector_x**2 + line_vector_y**2)
    # 计算矢量场制导律的角度
    k = 0.1
    ang_ = -math.atan(k * distance_to_line) + math.atan2(line_vector_y, line_vector_x)
    return ang_

def calculate_metrics(yaw, forward_v, lateral_v, origin_x, origin_y, hangdian2_x, hangdian2_y, boat_x, boat_y):
    yaw_rad = math.radians(yaw)

    # 计算船在指定向量方向上的速度投影
    # vector_hangdian = np.array([hangdian2_x - origin_x, hangdian2_y - origin_y])

    # v_x = np.array([forward_v * math.cos(yaw_rad), forward_v * math.sin(yaw_rad)])
    # v_y = np.array([lateral_v * math.cos(yaw_rad), lateral_v * math.sin(yaw_rad)])
    # velocity_projection = (np.dot(v_x, vector_hangdian) / math.sqrt(np.dot(vector_hangdian, vector_hangdian)) )
    #                        # + np.dot(v_y, vector_hangdian) / math.sqrt(np.dot(vector_hangdian, vector_hangdian)))

    # 计算船到指定直线的距离
    line_vector_x = hangdian2_x - origin_x
    line_vector_y = hangdian2_y - origin_y
    boat_vector_x = boat_x - origin_x
    boat_vector_y = boat_y - origin_y
    distance_to_line = (line_vector_x * boat_vector_y - line_vector_y * boat_vector_x) / math.sqrt(line_vector_x**2 + line_vector_y**2)
    # 计算矢量场制导律的角度
    k = 0.1
    ang_ = -math.atan(k * distance_to_line) + math.atan2(line_vector_y, line_vector_x)
    # 计算ang_和yaw_rad的夹角，限制在-pi到pi之间
    angle_error = ang_ - yaw_rad

    # 将角度误差调整到 -π 到 π 之间
    angle_error = (angle_error + math.pi) % (2 * math.pi) - math.pi

    # 计算船的艏向角度与指定向量的角度误差
    # vector_angle = math.radians(math.atan2(vector_y, vector_x))
    # if vector_angle < 0 :
    #     vector_angle += 2 * math.pi
    # angle_error = vector_angle - yaw_rad
    # if angle_error > math.pi:
    #     angle_error = angle_error - 2 * math.pi
    # elif angle_error < -math.pi:
    #     angle_error = angle_error + 2 * math.pi
    # angle_error = angle_error

    return distance_to_line, angle_error


def reward_function(distance_error, angle_error, turns_deque, done):
    # 确定各项权重，可以根据需要调整
    # weight_t_error = 0.2
    # weight_vel = 0
    # weight_ang_error = 0.5
    # print(t_error, vel, ang_error)
    # 计算奖励
    # reward = -0.5*abs(t_error)
    # reward = -weight_t_error * abs(t_error) + weight_vel * vel - weight_ang_error * abs(ang_error)
    # if abs(t_error)<0.5:
    #     reward += 1
    # if abs(ang_error)<0.18:
    #     reward += 1
    # if vel >= 2:
    #     reward += 10
    # if abs(t_error) > 10:
    #     reward = reward - 10
    #     done = 1

    k1 = 0.15 #[-90,90]
    k2 = 0.2 #[-10,10]
    k3 = 3  #[-1,1]

    w1 = 0.6
    w2 = 0.2
    w3 = 0.2


    angle_error = math.degrees(angle_error)
    if abs(angle_error) <= 90:
        r1 =  math.pow(math.e, -k1 *abs(angle_error))
    elif angle_error > 90 :
        r1 = -math.pow(math.e, -k1 *abs(angle_error-180))
    elif angle_error < -90 :
        r1 = -math.pow(math.e, -k1 *abs(angle_error+180))
    r2 = math.pow(math.e, -k2 *abs(distance_error))
    if len(turns_deque) != 1:
        std_dev = np.std(turns_deque)
        r3 = math.pow(math.e, -k3 * abs(std_dev))
    else:
        r3 = 0

    # print(forward_v,r4)

    total_reward =w1* r1 +w2 * r2 + w3 * r3

    if abs(distance_error) > 30:
        done = 1
    return total_reward, done

class USV_ActionSpace(object):
    def __init__(self, action_dim=2, action_low=-1.0, action_high=1.0):
        self.action_dim = action_dim
        self.action_low = action_low
        self.action_high = action_high

# 随机生成（0，0）附近30米范围内的航点
def random_target():
    target_x = 0
    target_y = 0
    while (target_x ** 2 + target_y ** 2) < 36:
        target_x = np.random.uniform(-15, 15)
        target_y = np.random.uniform(-15, 15)
    return target_x, target_y


# 计算两点之间的距离

# 获取直线 与 点的垂足
def get_foot(start_point, end_point, boat_position_x, boat_position_y):
    start_x, start_y = start_point
    end_x, end_y = end_point
    pa_x, pa_y = boat_position_x, boat_position_y

    p_foot = [0, 0]
    if start_point[0] == end_point[0]:
        p_foot[0] = start_point[0]
        p_foot[1] = pa_y
        return p_foot

    k = (end_y - start_y) * 1.0 / (end_x - start_x)
    a = k
    b = -1.0
    c = start_y - k * start_x
    p_foot[0] = int((b * b * pa_x - a * b * pa_y - a * c) / (a * a + b * b))
    p_foot[1] = int((a * a * pa_y - a * b * pa_x - b * c) / (a * a + b * b))

    return p_foot


def normalize(values, lower_limits, upper_limits):
    normalized_values = []
    for val, lower, upper in zip(values, lower_limits, upper_limits):
        normalized_val = (2 * (val - lower) / (upper - lower)) - 1
        normalized_values.append(normalized_val)
    return normalized_values

def write_data_to_file(data, filename):
    with open(filename, 'a') as file:
        file.write(data + '\n')